/* @file state_monitor.h
 * @brief This file contains the declaration of the ModeMonitor class.
 *
 * This file provides the declaration of the ModeMonitor class, which is used to
 * Per node status and alarms. The class definition includes a constructor and
 * some member functions.
 *
 * @author haibo.yang
 * @date 2023-08-09
 */
#pragma once

#include "state_monitor/monitor.h"
#include <realtime_tools/realtime_publisher.hpp>
#include <agv_msgs/msg/periph_status.hpp>
#include <agv_msgs/msg/pose_with_confidence.hpp>
#include <agv_msgs/msg/agv_mode.hpp>
#include <agv_msgs/msg/nav_type.hpp>
#include <agv_msgs/msg/agv_event_status.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include <agv_srvs/srv/change_cur_floor.hpp>
#include <agv_srvs/srv/controller_enable.hpp>
#include <agv_srvs/srv/set_operation_mode.hpp>
#include <agv_srvs/srv/change_nav_type.hpp>
#include <functional>

#define MODEKEYCOUNT 2
using KeyPressHandler = std::function<void(const std::string&, const rclcpp::Time, const bool)>;

namespace monitor
{
    enum PreOperationStatus
    {
        NO_ERROR = 0,
        ERROR
    };

    const std::string keyname[MODEKEYCOUNT] = {"manulAutoBtn", "modeButton"};

    struct io_input_state {
        std::string name;           
        bool state;                 
        bool last_state;            
        bool triggered;             
        float long_press_threshold;
        float short_press_threshold;
        rclcpp::Time last_press_time;  

        KeyPressHandler handler_; 

        io_input_state():name(""),
                        state(false),
                        last_state(false),
                        triggered(false),
                        long_press_threshold(1.0f),
                        short_press_threshold(0.02f),
                        last_press_time(Monitor::GetTimes()),
                        handler_(nullptr) {}

    };

    // A monitor which summarize other monitors' result and publish the whole status
    // if it has changed.
    class ModeMonitor : public Monitor
    {
    public:
        typedef agv_srvs::srv::SetOperationMode::Request MODE_REQUEST;

        ModeMonitor(const double interval, const bool mode=false):
                Monitor("ModeMonitor", interval, mode),
                cur_mode_(Mode_Manul),
                pre_mode_(Mode_Idle),
                last_mode_(Mode_Manul),
                init_mode_(Mode_Manul),
                event_status_(),
                periph_status_(),
                joystick_lock_(false),
                shielded_peripherals_status_(false),
                cur_nav_type_(0),
                last_nav_type_(0),
                location_time_(Monitor::GetTimes()),
                last_request_(Monitor::GetTimes()),
                initialize_agv_mode_(false),
                location_is_running_(false),
                has_located_(false),
                manual_automatic_button_trigger_(false),
                chassis_enable_state_(true) {}

        ~ModeMonitor();

        /// @brief run 
        void RunOnce(const rclcpp::Time current_time) override;

        /// @brief init
        void init() override;
    private:
        /// @brief update node status
        void UpdateStatus(const rclcpp::Time time);

        /// @brief operation mode change callback
        bool OperationModeCallback(const std::shared_ptr<agv_srvs::srv::SetOperationMode::Request> req, 
            std::shared_ptr<agv_srvs::srv::SetOperationMode::Response> res);

        /// @brief Navtype callback
        bool ChangeNavTypeCallback(const std::shared_ptr<agv_srvs::srv::ChangeNavType::Request> req, 
            std::shared_ptr<agv_srvs::srv::ChangeNavType::Response> res);

        /// @brief curFloor callback
        bool ChangeCurFloorCallback(const std::shared_ptr<agv_srvs::srv::ChangeCurFloor::Request> req, 
            std::shared_ptr<agv_srvs::srv::ChangeCurFloor::Response> res);

        /// @brief thread mode publish funtion
        void ModePublishThread();

        /// @brief operation mode change
        void SwitchMode(const rclcpp::Time time);

        /// @brief check event state
        void CheckEventState();

        /// @brief agv mode publish
        void AgvModePublish();

        /// @brief periph node callback
        void PeriphStatusCallBack(const agv_msgs::msg::PeriphStatus::ConstSharedPtr &msg);

        /// @brief Locating data callback
        void PoseCallBack(const agv_msgs::msg::PoseWithConfidence::ConstSharedPtr &msg);

        /// @brief io input callback
        void ReadInput();
        // bool setLastMode();

        /// @brief 获取当前导航模式
        int getNavType();

        /// @brief set navgate type
        bool setNavType(int nNavType);

        /// @brief read ros global param  string : current floor; int:floor number
        std::tuple<std::string, int> getFloorParam();

        /// @brief set current floor
        bool setCurFloor(std::string floorName);

        /// @brief Init Agv mode
        bool InitAgvMode();

        /// @brief save Agv mode
        void SaveAgvMode();

        /// @brief write to init file
        void writeInitFile(const int type, const Mode mode, const std::string floor);

        /// @brief trigger event
        void tiggerEvent(const int action, const int type, const bool ackreq, const int ecode,
                         const int param1, const int param2, const std::string msg, const rclcpp::Time time);

        /// @brief handler button event
        void on_button_pressed(const std::string& name, const rclcpp::Time time, const bool state);

        /// @brief call chissis enable sevice 
        void EnableChissisController(const rclcpp::Time time);

        void WaitForService()
        {
            while (!chassis_controller_client_->wait_for_service(std::chrono::seconds(1))) {
                if (!rclcpp::ok()) {
                    MONITOR_ERROR("[ModeMonitor] Interrupted while waiting for the service. Exiting.");
                    return;
                }
                MONITOR_INFO("[ModeMonitor] Service not available, waiting again...");
            }
            MONITOR_INFO("[ModeMonitor] Service is available.");
        }

    private:
        /// @brief publish current mode
        std::shared_ptr<rclcpp::Publisher<agv_msgs::msg::AGVMode>> agv_mode_pub_;
        std::shared_ptr<realtime_tools::RealtimePublisher<agv_msgs::msg::AGVMode>> realtime_agv_mode_pub_;

        /// @brief publish navgate mode
        std::shared_ptr<rclcpp::Publisher<agv_msgs::msg::NavType>> nav_type_pub_;

        /// @brief ros service operation mode
        rclcpp::Service<agv_srvs::srv::SetOperationMode>::SharedPtr operation_mode_;
        rclcpp::Service<agv_srvs::srv::ChangeNavType>::SharedPtr navigate_type_;
        rclcpp::Service<agv_srvs::srv::ChangeCurFloor>::SharedPtr cur_floor_server_;

        /// @brief current mode, pre mode
        Mode cur_mode_, pre_mode_, last_mode_, init_mode_;

        /// @brief pre operation state
        std::pair<rclcpp::Time, PreOperationStatus> event_status_, periph_status_;

        /// @brief subscriber periph status topic
        rclcpp::Subscription<agv_msgs::msg::PeriphStatus>::SharedPtr periph_status_sub_;
        rclcpp::Subscription<agv_msgs::msg::PoseWithConfidence>::SharedPtr pose_sub_;

        /// @brief event publish thread
        boost::thread mode_pub_thread_;

        /// @brief joystick lock state
        bool joystick_lock_;

        /// @brief Ignore peripheral status
        bool shielded_peripherals_status_;

        /// @brief Auto save timer
        rclcpp::TimerBase::SharedPtr mode_save_timer_;

        /// @brief Timed saving of agv mode parameters
        rclcpp::Time location_time_, last_request_;

        /// @brief After the thread starts, it will delay for a period of time
        rclcpp::Duration start_delay_time_{0, 0};

        /// @brief file path
        std::string file_name_;

        /// @brief navigate type
        int cur_nav_type_, last_nav_type_, init_nav_type_;

        /// @brief last input state
        io_input_state mode_key_input_[MODEKEYCOUNT];

        /// @brief thread run
        bool initialize_agv_mode_;

        /// @brief location node running
        bool location_is_running_;

        /// @brief Whether it has been located
        bool has_located_;

        /// @brief Define call servo disable service;
        rclcpp::Client<agv_srvs::srv::ControllerEnable>::SharedPtr chassis_controller_client_;

        /// @brief chassis servo enablbe state 2024.11.12
        bool manual_automatic_button_trigger_;

        /// @brief chassis servo enablbe state 2024.11.12
        bool chassis_enable_state_;

        /// @brief chassis request lock mutex;
        std::mutex mutex_;

        /// @brief estop last state
        int estop_last_state_;
        
        /// @brief wait service server thread
        std::thread service_wait_thread_;

        /// @brief cross floor param
        struct Floor {
            std::string last_floor;
            std::vector<std::string> floors;
            Floor() {
                last_floor = "unkown";
                num = 0;
                floors.clear();
            }

            void resize(std::size_t size) {
                if (num != size) {
                    floors.resize(size);
                    for (int i = 0; i < size; i++) {
                        floors[i] = std::to_string(i + 1) + "_Floor";
                    }
                    num = size;
                }
            }

        private:
            int num;
        } floor_;
    };

} // namespace monitor
